#include "CollisionAvoidance.h"
#include "../Character.h"
#include "../Vectores.h"
#include "../Obstacle.h"

#include <cfloat>
#include <vector>

using namespace std;

CollisionAvoidance::CollisionAvoidance(){
    maxAcceleration = 100 ;
    radius          = 300;
}

Steering* CollisionAvoidance::getSteering(){
    Steering *ret = new Steering();
    /*
    float minSeparation;
    float distance, firstDistance,
          relativePos[3], firstRelativePos[3],
          relativeVel[3], firstRelativeVel[3],
          relativeSpeed,
          timeToCollision, shortestTime;
    int firstTarget = -1;
    shortestTime = FLT_MAX;
    Kinematic * target;
    Steering* ret = new Steering();
    for (unsigned int i = 0, n = characters->size() ; i < characters->size();i++){
      // float aux[3];
      target = characters[i].getKinematic();
      restarVectores(target->position,character->position,relativePos);
      restarVectores(target->velocity,character->velocity,relativeVel);
      relativeSpeed = length(relativeVel);
      timeToCollision = multVectores(relativePos,relativeVel)/(relativeSpeed*relativeSpeed);
//        timeToCollision = (length(relativeVel)*length(relativePos)*((relativePos[0]* relativeVel[0] + relativePos[1]*relativeVel[1])/ )
      distance = length(relativePos);
      minSeparation = distance-relativeSpeed*shortestTime;
      if minSeparation > 2 * radius
        continue;
      if (timeToCollision > 0 && timeToCollision < shortestTime){
        shortestTime = timeToCollision;
        firstTarget = i;
        firstMinSeparation = minSeparation;
        firstDistance = distance;
        firstRelativePos = relativePos;
        firstRelativeVel = relativeVel;
      }
    }
    if (firstTarget == -1)
      return ret;
    if (firstMinSeparation <= 0  || distance <  2*radius){
      relativePos = 
    }
    */

    return ret;
}